dgit.raspbian.org Git - pcl.git/blob

   1 /*
   2  * shot_local_estimator.h
   3  *
   4  *  Created on: Mar 24, 2012
   5  *      Author: aitor
   6  */
   7
   8 #pragma once
   9
  10 #include <pcl/apps/3d_rec_framework/feature_wrapper/local/local_estimator.h>
  11 #include <pcl/apps/3d_rec_framework/feature_wrapper/normal_estimator.h>
  12 #include <pcl/features/fpfh_omp.h>
  13
  14 namespace pcl
  15 {
  16   namespace rec_3d_framework
  17   {
  18     template<typename PointInT, typename FeatureT>
  19       class FPFHLocalEstimationOMP : public LocalEstimator<PointInT, FeatureT>
  20       {
  21
  22         using PointInTPtr = typename pcl::PointCloud<PointInT>::Ptr;
  23         using FeatureTPtr = typename pcl::PointCloud<FeatureT>::Ptr;
  24         using KeypointCloud = pcl::PointCloud<pcl::PointXYZ>;
  25
  26         using LocalEstimator<PointInT, FeatureT>::support_radius_;
  27         using LocalEstimator<PointInT, FeatureT>::normal_estimator_;
  28         using LocalEstimator<PointInT, FeatureT>::keypoint_extractor_;
  29
  30       public:
  31         bool
  32         estimate (PointInTPtr & in, PointInTPtr & processed, PointInTPtr & keypoints, FeatureTPtr & signatures)
  33         {
  34
  35           if (!normal_estimator_)
  36           {
  37             PCL_ERROR("FPFHLocalEstimation :: This feature needs normals... please provide a normal estimator\n");
  38             return false;
  39           }
  40
  41           if (keypoint_extractor_.size() == 0)
  42           {
  43             PCL_ERROR("FPFHLocalEstimation :: This feature needs a keypoint extractor... please provide one\n");
  44             return false;
  45           }
  46
  47           //compute normals
  48           pcl::PointCloud<pcl::Normal>::Ptr normals (new pcl::PointCloud<pcl::Normal>);
  49           normal_estimator_->estimate (in, processed, normals);
  50
  51           //compute keypoints
  52           computeKeypoints(processed, keypoints, normals);
  53           std::cout << " " << normals->points.size() << " " << processed->points.size() << std::endl;
  54
  55           if (keypoints->points.size () == 0)
  56           {
  57             PCL_WARN("FPFHLocalEstimation :: No keypoints were found\n");
  58             return false;
  59           }
  60
  61           assert (processed->points.size () == normals->points.size ());
  62
  63           //compute signatures
  64           using FPFHEstimator = pcl::FPFHEstimationOMP<PointInT, pcl::Normal, pcl::FPFHSignature33>;
  65           typename pcl::search::KdTree<PointInT>::Ptr tree (new pcl::search::KdTree<PointInT>);
  66
  67           pcl::PointCloud<pcl::FPFHSignature33>::Ptr fpfhs (new pcl::PointCloud<pcl::FPFHSignature33>);
  68           FPFHEstimator fpfh_estimate;
  69           fpfh_estimate.setNumberOfThreads(8);
  70           fpfh_estimate.setSearchMethod (tree);
  71           fpfh_estimate.setInputCloud (keypoints);
  72           fpfh_estimate.setSearchSurface(processed);
  73           fpfh_estimate.setInputNormals (normals);
  74           fpfh_estimate.setRadiusSearch (support_radius_);
  75           fpfh_estimate.compute (*fpfhs);
  76
  77           signatures->resize (fpfhs->points.size ());
  78           signatures->width = static_cast<int> (fpfhs->points.size ());
  79           signatures->height = 1;
  80
  81           int size_feat = 33;
  82           for (std::size_t k = 0; k < fpfhs->points.size (); k++)
  83             for (int i = 0; i < size_feat; i++)
  84               signatures->points[k].histogram[i] = fpfhs->points[k].histogram[i];
  85
  86           return true;
  87
  88         }
  89
  90       };
  91   }
  92 }